///////////////////////////////////////////////////////////////////////////////
//
// Lynx 6 Arm Service - Configuration
//
///////////////////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Text;

using armproxy = Microsoft.Robotics.Services.ArticulatedArm.Proxy;
using Microsoft.Robotics.PhysicalModel.Proxy;
using physicalmodel = Microsoft.Robotics.PhysicalModel;

namespace ProMRDS.Robotics.Lynxmotion.Lynx6Arm
{
    class Lynx6ArmConfiguration : armproxy.ArticulatedArmState
    {            
        //measurements are in inches
        // TT Nov-2007 - Parameters seem to be wrong
        //const float BASE_HEIGHT = 3f;
        const float BASE_HEIGHT = 2.5f;
        const float ARM_LENGTH = 4.75f;
        //Model arm1 and arm4 with really short length since they can't be 0
        const float ARM_LENGTH2 = .001f;
        const float GRIPPER_LENGTH = 0f;
        const float ARM_THICKNESS = 1f;
        const float BASE_RADIUS = 2f;
        // TT - Add the Gripper
        const int _jointCount = 6;

        //The servo2 position at 1500 ms is offset from the y-axis
        public const float ANGLE2_OFFSET = (float) Math.PI / 9;

        float ARM_LENGTH_TOTAL = ARM_LENGTH + ARM_THICKNESS * 2;
        float ARM_LENGTH_TOTAL2 = ARM_LENGTH2 + ARM_THICKNESS * 2;

        // TT Nov-2007 -
        // Names for the joints
        // These names can be changed if desired.
        // Note carefully the order of the joints and make sure that
        // they are plugged into the corresponding servo outputs on
        // the SSC-32 controller. This does NOT correspond to the
        // order in the assembly manual for the arm.
        // If you are using an L5 arm then the WristRotate will not
        // work because the L5 does not have this feature.
        // Also, note that there is an enum for the joint/servo numbers
        // called JointNumbers in the arm types file.
        private string[] jointNames =
            {
                "Base",         // Servo 0
                "Shoulder",     // Servo 1
                "Elbow",        // Servo 2
                "Wrist",        // Servo 3
                "WristRotate",  // Servo 4
                "Gripper"       // Servo 5
            };

        //position is the origin of the global coordinate frame
        public Lynx6ArmConfiguration()
        {
            Joints = new List<Joint>();

            // all joints will be essentially of the same type:
            // A single DOF free, around the Joint Axis. A drive will be associated with
            // that DOF. Each joint will have a different joint axis and joint normal
            for (int i = 0; i < _jointCount; i++)
            {
                JointAngularProperties commonAngular = new JointAngularProperties();
                commonAngular.TwistMode = JointDOFMode.Free;

                commonAngular.TwistDrive = new JointDriveProperties();
                commonAngular.TwistDrive.Mode = JointDriveMode.Position;
                SpringProperties springProp = new SpringProperties();
                springProp.SpringCoefficient = 50000;
                springProp.DamperCoefficient = 10000;
                springProp.EquilibriumPosition = 0;
                commonAngular.TwistDrive.Spring = springProp;
                commonAngular.TwistDrive.ForceLimit = 1000000;

                //Each joint only has PI degrees of rotational range
                commonAngular.LowerTwistLimit = new JointLimitProperties();
                commonAngular.LowerTwistLimit.LimitThreshold = (float) -Math.PI / 2;
                commonAngular.UpperTwistLimit = new JointLimitProperties();
                commonAngular.UpperTwistLimit.LimitThreshold = (float) Math.PI / 2;

                Joint j = new Joint();
                JointProperties properties = new JointProperties();
                properties.Angular = commonAngular;
                // TT - Change the joint naming convention to be easier to use
                //properties.Name = "Joint" + i.ToString();
                properties.Name = jointNames[i] + "-Joint" + i.ToString();
                properties.Connectors = new EntityJointConnector[2];
                j.State = properties;
                Joints.Add(j);               
            }

            //Offset angle2's joint limits by PI/9
            Joints[2].State.Angular.LowerTwistLimit.LimitThreshold = (float) -Math.PI / 2 - ANGLE2_OFFSET;
            Joints[2].State.Angular.UpperTwistLimit.LimitThreshold = (float) Math.PI / 2 - ANGLE2_OFFSET;

            // programmatically build articulated arm
            CreateBase(Joints[0]);
            CreateArm1(Joints[0]);
            CreateArm2(Joints[1]);
            CreateArm3(Joints[2]);
            CreateArm4(Joints[3]);
            CreateArm5(Joints[4]);
            // TT - Added the gripper
            CreateArm6(Joints[5]);
        }

        #region Link creation
        void CreateBase(Joint joint)
        {
            float BASE_TOTAL = BASE_RADIUS * 2 + BASE_HEIGHT + ARM_LENGTH_TOTAL2;

            // create connect point for base. Top and center of its shape.
            EntityJointConnector connector = new EntityJointConnector();
            connector.JointNormal = new Vector3(1, 0, 0);
            // joint rotates around the Y(vertical axis)
            connector.JointAxis = new Vector3(0, 1, 0);
            //connector.JointConnectPoint = new Vector3(0, BASE_TOTAL, 0);
            connector.JointConnectPoint = new Vector3(0, BASE_HEIGHT, 0);

            joint.State.Connectors[0] = connector;
        }

        void CreateArm1(Joint joint)
        {
            EntityJointConnector connector = new EntityJointConnector();
            connector.JointNormal = new Vector3(1, 0, 0);
            connector.JointAxis = new Vector3(0, 1, 0);
            connector.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[1] = connector;
        }

        void CreateArm2(Joint joint)
        {
            EntityJointConnector connector0 = new EntityJointConnector();
            connector0.JointNormal = new Vector3(0, 1, 0);
            connector0.JointAxis = new Vector3(0, 0, 1);
            connector0.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[0] = connector0;

            EntityJointConnector connector1 = new EntityJointConnector();
            connector1.JointNormal = new Vector3(0, 1, 0);
            connector1.JointAxis = new Vector3(0, 0, 1);
            connector1.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[1] = connector1;
        }

        void CreateArm3(Joint joint)
        {
            EntityJointConnector connector0 = new EntityJointConnector();
            connector0.JointNormal = new Vector3(0, 1, 0);
            connector0.JointAxis = new Vector3(0, 0, 1);
            connector0.JointConnectPoint = new Vector3(0, ARM_LENGTH_TOTAL, 0);

            joint.State.Connectors[0] = connector0;

            EntityJointConnector connector1 = new EntityJointConnector();
            connector1.JointNormal = new Vector3(0, 1, 0);
            connector1.JointAxis = new Vector3(0, 0, 1);
            connector1.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[1] = connector1;
        }

        void CreateArm4(Joint joint)
        {
            EntityJointConnector connector0 = new EntityJointConnector();
            connector0.JointNormal = new Vector3(-1, 0, 0);
            connector0.JointAxis = new Vector3(0, 0, 1);
            connector0.JointConnectPoint = new Vector3(0, ARM_LENGTH_TOTAL, 0);

            joint.State.Connectors[0] = connector0;

            EntityJointConnector connector1 = new EntityJointConnector();
            connector1.JointNormal = new Vector3(-1, 0, 0);
            connector1.JointAxis = new Vector3(0, 0, 1);
            connector1.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[1] = connector1;
        }

        void CreateArm5(Joint joint)
        {
            EntityJointConnector connector0 = new EntityJointConnector();
            connector0.JointNormal = new Vector3(-1, 0, 0);
            connector0.JointAxis = new Vector3(0, 1, 0);
            connector0.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[0] = connector0;

            EntityJointConnector connector1 = new EntityJointConnector();
            connector1.JointNormal = new Vector3(-1, 0, 0);
            connector1.JointAxis = new Vector3(0, 1, 0);
            connector1.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[1] = connector1;
        }

        // This is not really a joint as such - it is the gripper
        // However, it is operated by a servo motor so it has the
        // usual range of joint values
        void CreateArm6(Joint joint)
        {
            EntityJointConnector connector0 = new EntityJointConnector();
            connector0.JointNormal = new Vector3(-1, 0, 0);
            connector0.JointAxis = new Vector3(0, 1, 0);
            connector0.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[0] = connector0;

            EntityJointConnector connector1 = new EntityJointConnector();
            connector1.JointNormal = new Vector3(-1, 0, 0);
            connector1.JointAxis = new Vector3(0, 1, 0);
            connector1.JointConnectPoint = new Vector3(0, 0, 0);

            joint.State.Connectors[1] = connector1;
        }
        #endregion
    }

    public static class ProxyConversion
    {
        public static physicalmodel.Vector3 FromProxy(Vector3 source)
        {
            return new physicalmodel.Vector3(source.X, source.Y, source.Z);
        }

        public static physicalmodel.Quaternion FromProxy(Quaternion source)
        {
            return new physicalmodel.Quaternion(source.X, source.Y, source.Z, source.W);
        }

        public static physicalmodel.AxisAngle FromProxy(AxisAngle source)
        {
            return new physicalmodel.AxisAngle(FromProxy(source.Axis), source.Angle);
        }

        public static Vector3 ToProxy(physicalmodel.Vector3 source)
        {
            return new Vector3(source.X, source.Y, source.Z);
        }

        public static Quaternion ToProxy(physicalmodel.Quaternion source)
        {
            return new Quaternion(source.X, source.Y, source.Z, source.W);
        }

        public static AxisAngle ToProxy(physicalmodel.AxisAngle source)
        {
            return new AxisAngle(ToProxy(source.Axis), source.Angle);
        }
    }
}
